#include "wingbot_client.hpp"
#include "data_holder.h"
#include "simple_dataframe.h"

#pragma once

// #define DEBUG_MSG

class WingbotMaster : public Dataframe
{
    struct FLAG{
        bool Velocity;
        bool Arm;
        bool Raise;
        bool ClearOdom;
        bool Spin;
    };

private:
    bool alive;
    Data_holder *dh;
    Message active_rx_msg;
    RECEIVE_STATE recv_state;
    WingClient *trans;
    uint32_t frame_sent;
    uint32_t frame_recvd;
    uint8_t uuid; // 0x21~0x7E
    struct FLAG flags;

public:
    WingbotMaster(WingClient *trans = 0) : trans(trans)
    {
        recv_state = STATE_RECV_FIX;
        frame_sent = 0;
        frame_recvd = 0;
        dh = Data_holder::get();
        alive = true;
        uuid = 0x21;
        memset(&flags, 0, sizeof(flags));
    }

    ~WingbotMaster()
    {
        alive = false;
    }

    uint8_t getFrameUuid() { return uuid; }
    bool is_alive() { return alive; }

    bool init()
    {
        if (!trans->connect())
            return false;

        trans->wait_data();
        boost::thread([&]() { this->loop(); });
        return true;
    }

    RobotState_T getState()
    {
        return dh->robot_state;
    }

    /**
     * ************** ROBOT CONTROL ********************
     */

    /**
     * @brief set robot speed
     * @param vx mm/s
     * @param vy mm/s
     * @param rz 0.001rad/s
     * @return true command set success
     */
    bool set_speed(float vx, float vy, float rz)
    {
        dh->velocity.v_liner_x = vx;
        dh->velocity.v_liner_y = vy;
        dh->velocity.v_angular_z = rz;
        flags.Velocity = true;
        return true;
    }

    bool set_hydraulic_arm(const SetHydrauArm_T* arm)
    {
        memcpy(&dh->hydraulic_arm, arm, sizeof(dh->hydraulic_arm));
        flags.Arm = true;
        return true;
    }

    bool set_hydraulic_raise(const SetHydroRaise_T* raise)
    {
        memcpy(&dh->hydraulic_raise, raise, sizeof(dh->hydraulic_raise));
        flags.Raise = true;
        return true;
    }

    bool set_spin(const SetSpin_T* spin)
    {
        memcpy(&dh->robot_spin, spin, sizeof(dh->robot_spin));
        flags.Spin = true;
        return true;
    }

    /**
     * @brief set odom x, y, yaw to zero
     * @return true command set success
     */
    bool clear_odom(void)
    {
        flags.ClearOdom = true;
        return true;
    }

    /********************* END OF ROBOT CONTROL ********************************/

    bool request(const MESSAGE_ID id, int retry = 3)
    {
        int max_cnt = retry;
        for(;;)
        {
            if(interact(id))
                return true;

            if(max_cnt-- == 0)
                break;
        }
        return false;
    }

    void loop()
    {
        static int loop_cnt = 0;
        std::cout << "sleep 1s." << std::endl;
        boost::this_thread::sleep_for(boost::chrono::seconds(1));
        std::cout << "wingbot master loop start" << std::endl;
        while (alive)
        {
            //boost::this_thread::sleep_for(boost::chrono::milliseconds(10));
            if (trans->ready())
            {
#ifdef DEBUG_MSG
                std::cout << "frame sent: " << frame_sent << ", frame received: " << frame_recvd << std::endl;
#endif
                request(ID_GET_ROBOT_STATE);
                request(ID_GET_ODOM);
                if(flags.Velocity){ request(ID_SET_VELOCITY);flags.Velocity = false;}
                if(flags.Arm){ request(ID_SET_HYDRAU_ARM);flags.Arm = false; }
                if(flags.Raise){ request(ID_SET_HYDRAU_RAISE);flags.Raise = false;}
                if(flags.Spin){ request(ID_SET_SPIN);flags.Spin = false;}
                if(flags.ClearOdom){ request(ID_INIT_ODOM);flags.ClearOdom = false; }
                loop_cnt = 0;
            }
            else
            {
                if (loop_cnt++ > 10)
                {
                    loop_cnt = 0;
                    trans->connect();
                    std::cout << "[Warn] SerialPort Closed, Trying to ReOpen" << std::endl;
                }
            }
        }
    }

private:
    void register_notify(const MESSAGE_ID id, Notify *_nf)
    {
    }

    bool data_recv(unsigned char c)
    {

#ifdef DEBUG_MSG
        if (c == FIX_HEAD)
            printf("[HEAD] ");
        printf("%02X ", c);
#endif

        switch (recv_state)
        {
        case STATE_RECV_FIX:
            if (c == FIX_HEAD)
            {
                memset(&active_rx_msg, 0, sizeof(active_rx_msg));
                active_rx_msg.head.flag = c;
                active_rx_msg.check += c;
                recv_state = STATE_RECV_ID;
            }
            else
                recv_state = STATE_RECV_FIX;
            break;
        case STATE_RECV_ID:
            if (c < ID_MESSGAE_MAX)
            {
                active_rx_msg.head.msg_id = c;
                active_rx_msg.check += c;
                recv_state = STATE_RECV_LEN;
            }
            else
                recv_state = STATE_RECV_FIX;
            break;
        case STATE_RECV_LEN:
            active_rx_msg.head.length = c;
            active_rx_msg.check += c;
            if (active_rx_msg.head.length == 0)
                recv_state = STATE_RECV_CHECK;
            else
                recv_state = STATE_RECV_DATA;
            break;
        case STATE_RECV_DATA:
            active_rx_msg.data[active_rx_msg.recv_count++] = c;
            active_rx_msg.check += c;
            if (active_rx_msg.recv_count >= active_rx_msg.head.length)
                recv_state = STATE_RECV_CHECK;
            break;
        case STATE_RECV_CHECK:
            if (active_rx_msg.check == c)
            {
                recv_state = STATE_RECV_END;
            }
            else
            {
                recv_state = STATE_RECV_FIX;
            }
            break;
        case STATE_RECV_END:
#ifdef DEBUG_MSG
            printf("[END]\n");
#endif
            recv_state = STATE_RECV_FIX;
            if (END_LEGAL(c))
            {
                active_rx_msg.end_frame = c;
                return true;
            }
            break;
        default:
            recv_state = STATE_RECV_FIX;
            break;
        }

        return false;
    }

    int data_parse()
    {
        MESSAGE_ID id = (MESSAGE_ID)active_rx_msg.head.msg_id;

#ifdef DEBUG_MSG
        printf("[data_parse]: id = %02X\r\n", id);
#endif

        switch (id)
        {
        case ID_GET_VERSION:
            memcpy(&dh->firmware_info, active_rx_msg.data, sizeof(dh->firmware_info));
            dh->msg_flags.version = true;
            break;
        case ID_SET_ROBOT_PARAMTER:
            break;
        case ID_GET_ROBOT_PARAMTER:
            memcpy(&dh->parameter, active_rx_msg.data, sizeof(dh->parameter));
            break;
        case ID_INIT_ODOM:
            break;
        case ID_SET_VELOCITY:
            break;
        case ID_GET_ODOM:
            memcpy(&dh->odom, active_rx_msg.data, sizeof(dh->odom));
            dh->msg_flags.odom = true;
            break;
        case ID_GET_ROBOT_STATE:
            memcpy(&dh->robot_state, active_rx_msg.data, sizeof(dh->robot_state));
            dh->msg_flags.robot_state = true;
            break;
        default:
            break;
        }

        return active_rx_msg.end_frame;
    }

    bool interact(const MESSAGE_ID id)
    {
        int cnt = 0;
        frame_sent++;
        switch (id)
        {
        case ID_GET_VERSION:
            send_message(id);
            break;
        case ID_SET_ROBOT_PARAMTER:
            send_message(id, (unsigned char *)&dh->parameter, sizeof(dh->parameter));
            break;
        case ID_GET_ROBOT_PARAMTER:
            send_message(id);
            break;
        case ID_INIT_ODOM:
            send_message(id);
            break;
        case ID_SET_VELOCITY:
            send_message(id, (unsigned char *)&dh->velocity, sizeof(dh->velocity));
            break;
        case ID_GET_ODOM:
            send_message(id);
            break;
        case ID_GET_PID_DATA:
            send_message(id);
            break;
        case ID_GET_ROBOT_STATE:
            send_message(id);
            break;
        case ID_GET_IMU_DATA:
            send_message(id);
            break;
        case ID_SET_EACH_MOTOR:
            send_message(id, (unsigned char *)&dh->each_motor, sizeof(dh->each_motor));
            break;
        case ID_SET_HYDRAU_ARM:
            send_message(id, (unsigned char *)&dh->hydraulic_arm, sizeof(dh->hydraulic_arm));
            break;
        case ID_SET_HYDRAU_RAISE:
            send_message(id, (unsigned char *)&dh->hydraulic_raise, sizeof(dh->hydraulic_raise));
            break;
        case ID_SET_SPIN:
            send_message(id, (unsigned char *)&dh->robot_spin, sizeof(dh->robot_spin));
            break;
        case ID_RESET_WHEELS:
            send_message(id);
            break;
        case ID_SET_DOUBLE_CAR:
            send_message(id, (unsigned char *)&dh->double_car, sizeof(dh->double_car));
            break;
        case ID_SET_ODOM_TF:
            send_message(id, (unsigned char *)&dh->odom_tf, sizeof(dh->odom_tf));
            break;
        default:
            break;
        }

        for (;;)
        {
            if (uuid == wait_ack())
                return true;
            if (cnt++ > 5)
                break; // 500ms timeout
        }

        std::cout << "[read timeout]" << std::endl;
        return false;
    }

    bool send_message(const MESSAGE_ID id)
    {
        Message msg(id, &uuid);
        send_message(&msg);
        return true;
    }

    bool send_message(const MESSAGE_ID id, unsigned char *data, unsigned char len)
    {
        Message msg(id, &uuid, data, len);
        send_message(&msg);
        return true;
    }

    bool send_message(Message *msg)
    {
        if (trans == 0)
            return true;
        std::vector<uint8_t> data((unsigned char *)msg, (unsigned char *)msg + sizeof(msg->head) + msg->head.length + 2); //modified by MLC, add end byte
        trans->write(data);
        return true;
    }

    int wait_ack(uint32_t timeout = 100)
    {
        uint16_t data;
        uint8_t useful_data;
        trans->set_timeout(timeout);

        for (;;)
        {
            if (trans->available())
            {
                data = trans->read();
                if (data < 0x0100)
                {
                    useful_data = data & 0x00FF;
                    if (data_recv(useful_data))
                    {
                        int ret = data_parse();
                        frame_recvd++;
                        return ret;
                    }
                }
            }

            if (trans->is_timeout())
            {
                return 0xFF;
            }
        }

        return 0xFF;
    }
};